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Abstract  The  ability  to  mathematically  model  the  movement  of  a  robot  manipulator  is  a 

prerequisite  to  the  understanding  of  the  key  factors  that  influence  a  manipulator’s 
performance.  This  paper  presents  a  manipulator  model  which  has  been  used  to  simulate 
and  control  a  real  robot  arm.  A  method  of  describing  the  arm  by  its  rotational 
characteristics,  a  set  of  equations  called  the  Inverse  Arm,  and  an  algorithm  called  the 
Forward  Arm  are  presented.  The  Forward  Arm  simulates  the  movement  of  an  arm  and  the 
Inverse  Arm  provides  a  means  of  computing  the  correct  voltages  to  apply  to  an  arm  to 
achieve  a  desired  movement. 
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Notation: 

•  The  vector  sign  (  )  indicates  a  three  dimensional  spatial  vector. 

•  An  underbar  beneath  a  symbol  indicates  an  "arm  vector”  which  has  one  component  for  each  joint 
in  the  arm.  For  example.  S.  is  equal  to  ( 0 v02<-<^ N )  when  there  are  N  joints  in  the  arm. 

•  Subscripts  indicate  one  of  two  things: 

1.  The  coordinate  frame  in  which  the  symbol  is  referenced  and  the  frame  to  which  the  symbol 
refers. 

2.  The  joint  number  that  is  referred  to  by  the  symbol. 

The  type  of  symbol  will  determine  whether  the  subscript  is  a  link  coordinate  frame  or  a  joint 
number.  For  example,  w.  is  the  angular  velocity  of  link  /  in  the  link  i  coordinate  system  and  0.  is 
the  position  of  joint  i. 

a 

•  A  hat  on  a  symbol  t  }  indicates  a  constant 

•  A  star  superscript  ( * )  indicates  a  value  related  to  the  center  of  mass.  Other  values  are  related  to 
the  link  coordinate  frame. 

•  A  dot  and  a  double  dot  over  a  variable  (  and  )  indicate  first  and  second  time  derivatives 
respectively. 

•  An  A  with  a  superscript  and  a  subscript  represents  a  transformation  matrix  from  the  superscripted 
coordinate  system  to  the  subscripted  coordinate  system;  so  o>2  is  the  angular  velocity  of  the  link 
2  coordinate  origin  in  link  1  coordinates. 

•  Boldfaced  letters  represent  matrices  or  tensors,  so  J  is  a  moment  of  inertia  tensor. 

Note  that  a  reference  to  the  velocity  or  acceleration  of  a  link  actually  refers  to  the  velocity  or  acceleration  of 
the  link’s  coordinate  system  origin. 
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Introduction 

A  mathematical  model  of  a  physical  system,  such  as  a  robot  manipulator(arm),  is  one  of  the  most  useful 
tools  available  for  studying  the  system’s  behavior.  The  model,  usually  in  the  form  of  a  computer  program,  can 
be  used  to  study  the  system  in  several  ways.  The  development  of  an  accurate  model  leads  to  a  full 
understanding  of  all  of  the  key  elements  of  the  system.  The  model  provides  a  means  of  testing  the  system 
under  conditions  that  would  be  dangerous  or  impossible  for  the  real  system.  Larger  systems  that  contain  the 
modeled  system  can  be  tested  with  the  model  instead  of  the  real  system. 

As  part  of  the  Camegie-Mcllon  University  Direct-Drive  Manipulator  Project  (CMU  DD  Arm  Project)  we 
have  developed  a  mathematical  model  of  the  manipulator.  The  mathematical  equations  arc  based  on  a 
Newton-Euler  analysis  of  firee-body  dynamics  developed  for  robotic  manipulators.  [8]  [11] 

This  paper  describes  the  structure  of  the  model  that  simulates  the  dynamic  motions  of  our  manipulator. 
The  model  is  divided  into  three  parts. 

•  A  detailed  description  of  the  structure  of  the  amt  The  description  of  the  structure  is  contained  in  a 
Manipulator  Database  which  consists  of  two  parts:  the  kinematic  and  the  dynamic.  The  kinematic 
description  specifies  the  relative  positions  between  the  links  of  the  arm  and  gives  the  axes  of 
rotation  for  each  of  the  joints.  This  description  is  easily  determined  from  the  mechanical  drawings 
of  the  arm.  The  dynamic  description  contains  the  moment  of  inertia,  the  center  of  mass,  and  the 
mass  for  each  of  the  links.  A  computer  program  was  written  to  calculate  these  values  from  a 
database  file(the  Parts  Database)  that  contains  a  description  of  every  piece  of  the  arm. 

•  The  Inverse  Arm.  This  is  a  set  of  equations  which,  when  evaluated,  yield  the  motor  voltages 
required  to  produce  certain  accelerations.  This  is  the  inverse  of  a  real  arm  which  produces 
accelerations  given  the  voltages.  The  Inverse  Ami  part  of  the  model  is  needed  for  the  third  part 
which  is  the  Toward  Arm. 

•  The  Forward  Arm.  This  part  of  the  model  contains  an  algorithm  which  can  compute  values  for  the 
acceleration  of  the  joints  in  the  arm  when  the  motor  voltages  are  specified.  When  the 
accelerations  are  integrated  over  a  period  of  time,  the  new  positions  and  velocities  can  be 
determined. 

Arm  Description 

The  CMU  DD  Arm  consists  of  seven  links,  numbered  0  to  6,  going  from  the  base  (link  0)  down  to  the  hand 
(link  6).  There  are  six  joints  numbered  1  to  6.  The  odd  numbered  joints  are  rotational  joints  which  rotate  in 
the  same  manner  as  the  turning  of  a  screw.  The  even  numbered  joints  are  pivotal  joints,  which  move  in  a 
manner  similar  to  that  of  a  person’s  elbow. 

Each  link  has  a  coordinate  frame  associated  with  it  The  Denavit-Hartenburg  convention  [5]  for  assigning 
coordinate  frames  to  manipulator  links  is  used  to  specify  the  coordinate  frames  of  the  manipulator  because  it 
simplifies  the  evaluation  of  the  equations  used  in  the  Inverse  Arm  and  Forward  Arm  parts  of  the  model. 
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The  Denavit-Hartenburg  convention  specifies  that  link  i+ 1  rotates  around  the  Z  axis  of  link  /,  denoted  Zj, 
when  joint  i+  /  turns.  Link  1,  therefore,  rotates  around  ZQ  at  joint  1  and  link  2  around  at  joint  2,  etc.  The 
X  axis  of  each  link’s  coordinate  system  points  along  the  common  normal  of  the  link’s  Z  axis  and  the  Z  axis  of 
the  previous  link.  If  there  is  no  common  normal,  such  as  when  the  two  Z  axes  intersect,  the  direction  of  the  X 
axis  is  arbitrary,  so  long  as  it  is  perpendicular  to  the  Z  axis.  The  Y-axis  is  perpendicular  to  both  the  X  and  Z 
axes  and  completes  a  right-handed  coordinate  frame.  The  coordinate  frames  specified  by  the  Denavit- 
Hartenburg  convention  for  the  CMU  DD  Arm  are  shown  in  figure  1.  [4] 


Kinematic  Description 

The  kinematic  description  specifies  die  general  organization  of  a  manipulator.  The  description  is  a 
database  which  contains  three  pieces  of  information,  denoted  a,  r,  and  a,  for  each  joint  For  '.ny  joint  i,  a 
specifics  the  angle  of  rotation  necessary  to  bring  Z{1  parallel  to  or  collinear  with  Z..  The  parameter  r  specifies 
the  distance  along  the  Z^  axis  from  the  link  /•/  coordinate  system  to  the  link  i  coordinate  system.  The  a 
parameter  specifies  the  distance  from  link  i-I  to  link  i  along  the  XH  axis.  Once  the  coordinate  frame  origins 
are  determined  in  the  Denavit-Hartenburg  convention  the  u,  r,  and  a  parameters  can  be  obtained  from  the 
mechanical  drawings. 


Dynamic  Description 

Obtaining  a  dynamic  description  of  the  arm  requires  a  greater  effort  than  the  kinematic  description.  The 
dynamic  description  consists  of  the  moment  of  inertia  tensor,  center  of  mass  vector,  and  the  mass  scalar  of 
each  link.  To  determine  this  data  we  have  developed  a  Parts  Database1  for  the  CMU-DD  Arm  which  has 
information  on  each  of  the  six  links.  Within  the  database,  each  link  is  separated  into  several  parts  which  are 
numbered  in  the  mechanical  drawings.  Each  part  is  broken  down  into  several  sections.  Each  section  is 
described  as  a  cylinder,  semi-cylinder,  rectangle,  sphere,  or  prism.  The  characteristics  of  each  of  the  parts  in 
our  manipulator  can  be  approximated  by  combinations  of  these  shapes.  The  database  contains  dimensions, 
densities,  and  locations  of  each  section  relative  to  the  part  that  it  belongs  to.  With  this  information  we  can 
determine  the  moment  of  inertia,  center  of  mass,  and  mass  of  each  section.  Once  the  information  is  calculated 
for  each  section,  it  can  then  be  determined  for  each  part  using  the  formulas  for  transforming  moments  of 
inertia.  [7]  [10j2  The  dynamic  description  of  each  link  can  be  computed  in  a  similar  manner  from  the  part 
information.  The  resultant  description  is  saved  as  part  of  the  Manipulator  Database  and  is  referred  to  by  the 
rest  of  the  arm  model.  It  must  be  recalculated  if  the  construction  of  the  arm  changes. 

There  are  other  pieces  of  data  related  to  the  dynamic  description  of  the  arm  which  can  only  be 
expcrimently  determined.  The  most  important  of  these  arc  the  motor  constants.  Each  motor  has  a  resistance, 
R,  and  a  torque  constant.  Kt.  The  resistance  of  the  motor  relates  the  current  in  each  motor’s  armature  to  the 
voltage  applied  across  each  motor’s  terminals.  The  torque  constant  relates  the  torque  produced  by  the  motor 
to  the  current  in  the  armature  and  it  relates  the  speed  of  the  motor  to  the  back  EM F( Electro- Motive  Force). 


The  link  coordinate  systems  used  in  the  parts  database  are  assigned  for  convenience  The  software  which  generates  the  dynamic 
description  changes  these  coordinate  systems  to  those  of  the  Dcnavit  Hartenburg  convention  when  the  Manipulator  Database  is 
produced. 

^Chapter  10.5  rite  Inertia  Tensor 
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Inverse  Arm 

The  Inverse  Arm  model  is  an  application  of  a  Newton-Euler  analysis  of  free-body  motion.  The  purpose  of 
this  model  is  to  allow  us  to  compute  the  motor  voltages  required  to  produce  given  accelerations  when  we 
know  the  current  state  of  the  arm  and  all  of  its  parameters.  The  model  has  six  major  steps. 

1.  Calculation  of  the  velocities  and  accelerations  of  each  of  the  links. 

2.  Finding  the  linear  acceleration  of  each  of  the  link’s  center  of  mass. 

3.  Computing  the  net  force  and  moment  exerted  on  each  link. 

4.  Calculation  of  the  local  forces  and  moments  on  each  link. 

5.  Finding  the  torque  required  for  each  motor. 

6.  Computing  the  motor  voltage  required  to  produce  the  computed  torques. 

The  last  step  is  done  separately  from  the  first  five  so  that  the  Forward  Arm  program  can  use  the  first  five  steps 
of  the  Inverse  Arm  to  find  torques. 

In  this  paper  a  reference  to  the  velocity  or  acceleration  of  a  link  actually  refers  to  the  velocity  or  acceleration 
of  the  coordinate  system  imbedded  in  the  link. 


Link  Velocities  and  Accelerations 

There  arc  two  forms  of  link  velocities  and  accelerations  which  are  considered  here,  angular  and  linear.  We 
have  four  equations  which  can  be  solved  iteratively  from  link  1  to  link  N  to  find  the  angular  velocity,  angular 
acceleration,  linear  velocity,  and  linear  acceleration  of  each  of  the  links  in  the  arm.  Link  0  is  assumed  to  have 

V  V 

no  angular  or  linear  velocity  and  no  angular  acceleration(i.e.  =  vQ  =  toQ  =  [0  0  0]  ).  It  does,  however, 
have  a  linear  acceleration  equal  to  a  Z  directed  gravitational  acceleration  (i.e.  v^  =  [0  0  g]T,  g  =  ±  9.80621 
meters/second2,  depending  upon  whether  Z  points  up  or  down).  Since  we  know  wQ,  v^,  and  v^  we  can  use 
the  following  four  equations  to  solve  for  tar  v^,  and  Vp  We  can  then  apply  the  equations  repeatedly  to 
solve  for  the  velocities  and  accelerations  of  links  2,  3,  etc.  up  through  link  N. 

The  angular  velocity  of  link  i+  /,  wj+1,  is  related  to  the  angular  velocity  of  link  /,  Z^,  and  the  rate  at  which 
the  joint  between  them,  0;+ p  turns  by 

*.♦1  =  A'i+i  <  «i  +  Vi+1>  i  =  0’l . N*1  (D 

where  Zq  =  [0  0  1]  ‘  and  N  =  6  in  the  CMU  DD  Arm.  The  Denavit-Hartenburg  convention  dictates  that  the 
axis  of  rotation  of  joint  i+ 1  is  along  the  Z  axis  of  the  link  /  coordinate  frame,  so  the  rate  of  turning  of  joint 
i+ 1  is  multiplied  by  ZQ  and  added  to  the  angular  velocity  of  link  i  to  give  the  angular  velocity  of  link  i+ 1. 
The  coordinate  frame  is  changed  from  link  i  to  link  i+ 1  by  premultipling  by  Ai+  r 

The  angular  acceleration  of  link  i+  /,  wj+1,  is  given  by: 
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A!+i  <  "i 


Vi+ 


1  +  «i  x  Vi+1  > 


i  =  0,1,...,N-1. 


(2) 


Since  joint  i+ 1  rotates  around  the  Z  axis  of  link  /,  the  acceleration  of  joint  i+1  is  multiplied  by  ZQ  before 
being  changed  to  link  i+1  coordinates.  The  cross  product  term  comes  from  the  geometry  of  the 
situation.  [10]3  A  coordinate  transformation  from  link  i  to  link  i+1  coordinates  is  performed  by  a 
premultiplication  by  Aj+1> 


The  linear  velocity  of  link  i+ 1 ,  vj+1,  is  related  to  the  linear  velocity  of  link  r,  v;,  and  the  cross  product  of 
«i+ 1  and  the  vector,  p  i+1,  which  points  from  the  link  i  to  the  link  i+ 1  coordinate  system. 

Vi  =  “i+ix  Pi+i  +  A!+l^  i  =  0-1 . N*1 

where  pj  +  1  is  given  by  [  aj+1  r  +  1sin(aj  +  1)  rj  +  1cos(ai  +  1)  ]T  in  the  Denavit-Hartenburg 
convention.  The  linear  velocity  of  the  link  /  coordinate  system  is  transformed  to  the  link  i+1  coordinate 
system  by  a  premultiplication  by  Aj+r  The  cross  product  need  not  be  transformed  because  it  is  already 
expressed  in  link  i+  I  coordinates.  Note  that  the  linear  velocity  of  each  link  is  not  used  in  later  calculations. 
This  equation  need  not  be  evaluated,  but  is  included  for  completeness. 


The  linear  acceleration  of  link  i+  /,  v*+  r  is  given  by 


v. 


i+1  +  “i+1 


+  Ai+1  v, 


i  =  0,1,...,N-1. 


(4) 


i  +  l  "  “i+ix  Pi+i  +  “i+i  x  <  “i+l  x  Pi+i> 

The  first  term  is,  again,  due  to  the  geometry'  of  the  situation  and  thp  second  is  called  the  Centripetal 
acceleration.  This  equation  is  a  limited  case  of  the  Coriolis  theorem.  [10]  Because  there  are  no  translational 
joints  in  cur  arm,  the  coriolis  term  of  the  Coriolis  theorem  is  zero. 


Linear  Acceleration  of  the  Centers  of  Mass 

The  calculation  of  the  linear  acceleration  of  the  center  of  mass  of  each  of  the  links  is  very  similar  to  the 
linear  acceleration  of  the  coordinate  system  calculation.  The  equation  relating  the  linear  acceleration  of  the 
center  of  mass  of  a  link  to  oa,  Uj,  s  t ,  and  v*  is 

vf  =  Uj  x  s’  +  Uj  x  (  5.  x  s’)  +  f  i  =  1,2 . N  (5) 

where  s  *  is  a  vector  pointing  to  the  center  of  mass  of  link  /  from  its  coordinate  origin.  Again,  we  see  that 
there  are  no  coriolis  accelerations  in  the  arm.  Note  that  these  calculations  can  be  performed  in  any  order,  but 
must  be  performed  after  equations  1  through  4  have  been  evaluated  for  all  of  the  links. 


Net  Forces  and  Moments 

The  net  force  is  the  sum  of  all  of  the  forces  acting  on  a  link.  Likewise,  the  net  moment  is  the  sum  of  all  of 
the  moments.  Since  we  know  what  the  accelerations  are  we  can  calculate  the  net  forces  and  net  moments  for 
each  link  using  Newton’s  law  and  its  analog  in  rotatiorfal  dynamics.  Newton's  law  in  this  context  is 

F.  =  m.t*  i  =  1,2, . N  (6) 

where  nr  is  the  mass  of  the  link. 


3 


Chapter  7  2  Moving  Origin  of  Coordinates 
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Newton’s  law  can  be  derived  from  the  fact  that  the  net  force  is  equal  to  the  rate  of  change  of  the 
momentum.  In  a  similar  manner,  we  can  derive  the  rotational  analogy  of  Newton’s  law  from  the  fact  that  the 
net  moment  is  equal  to  the  rate  of  change  of  the  rotational  momentum  or 

dL 

N.  =  — 

i 

dt 

where 

h  =  J,’*r 

L  is  the  rotational  momentum  and  J*  is  the  moment  of  inertia  of  link  i  around  its  center  of  mass.  Since  we 
need  to  express  the  moment  with  respect  to  link  coordinate  origins  we  change  the  coordin  of  the  moment 
The  moment  coordinate  change  formula  is: 

N(  =  J*  w.  +  Wj  x  (  J*  tSj  )  i  =  1,2,....  (7) 

This  is  a  form  of  Euler’s  equation  of  motion  for  a  rigid  body.  [10J4 


Local  Forces  and  Moments 

Each  link  is  connected  to  two  other  links  (except  the  hand  and  the  base)  which  exert  forces  on  that  link. 
The  sum  of  these  two  forcesjs  the  net  force.  For  any  link  /,  the  force,  f,  that  link  i-I  exerts  on  it  is  called  the 
local  force.  The  net  force,  Ft,  on  link  t  is  the  sum  of  the  local  force,  f,  and  the  negative  of  the  local  force  on 
the  next  link,  f  +  1,  that  is 


F. 


or 


r  =  A!+1  ff+1  +  F.  i  =  N,...,2,l.  (8) 

Note  that  we  must  change  the  coordinate  system  of  fj  +  1  before  adding  it  to  F.  by  premultiplying  it  by  Aj+1 
We  can  calculate  the  local  forces  by  solving  this  equation  iteratively  starting  at  the  hand,  where  fN+ x  is  the 
external  force  exerted  on  the  hand,  and  working  our  way  up  the  arm. 


The  net  moment,  Nj,  of  link  /  has  four  components. 

1.  The  local  moment  of  the  link,  nj,  which  is  the  moment  exerted  by  link  i-l  on  link  i. 


2.  The  negative  of  the  local  moment  of  the  next  link  transformed  to  the  link  /  coordinate  system,  that 
is 


3.  The  moment  caused  by  the  local  force  acting  on  the  link  at  a  distance  away  from  the  the  origin. 


Chapter  11.2  Euler's  liquations  of  Motion  fora  Rigid  Body 
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This  is  the  negative  of  the  cross  product  of  the  position  vector  which  points  from  the  /-/  to  the  i 
coordinate  origin  and  the  local  force  on  the  next  link  transformed  to  the  link  /  coordinate  system. 


4.  The  moment  caused  by  the  net  force  acting  on  the  center  of  mass  of  the  link. 

-<Pj+s*)xFr 

This  is  the  negative  of  the  cross  product  of  the  vector  pointing  from  the  link  i-1  coordinate  system 
origin  to  the  center  of  mass  of  link  i  and  the  net  force  on  link  /. 


By  rearranging  these  components  to  solve  for  the  local  moment  we  get 

a;  =  a|+1  +  p,  x  (A|+‘if+1)  +  ( p,  +  s')  x  f,  +  S, 

i  =  N,...,2,l. 

where  ru,  ,  is  the  external  moment  exerted  on  the  hand. 

iN  +  1 


(9) 


We  can  iteratively  solve  this  equation  from  the  hand  back  to  the  base  to  find  the  local  moments  on  each 
link. 


Joint  Torques 

The  local  moment  of  link  /  is  the  moment  that  the  link  exerts  on  joint  i-1.  The  component  of  the  local 
moment  that  is  along  the  Z(1  axis  is  the  torque  exerted  on  joint  i-1.  The  torque  required  for  a  joint  to 
compensate  for  the  local  moment  and  friction  is  given  by 

Tj  =  n.-(  A;*1^  )  +  \y.  b.  i  =  N-1,.,.,1  (10) 

where  h  is  the  friction  coefficient  of  joint  /.  The  friction,  bt,  in  each  of  the  joints  is  related  to  the  velocity  of 
the  joint  by  some  nonlinear  function.  Since  the  friction  in  the  joints  of  the  CMU  DD  arm  is  very  small,  we 
neglect  this  term  in  the  simulation.  [2]  [1] 


Defining  the  InvArm  Function 

We  can  define  the  function  which  evaluates  equations  1  through  10  as 


r  =  InvArm  (£,&>  8.) 


where r  =  (r 2,t 2 . tn),  1  =  (0p02,...,0N), §_  =  (d1,02 . 0N), and#  =  (0r02 . 0 N).  This  function  call  is 

an  actual  procedure  in  the  software  which  implements  the  algorithms  discussed  in  this  paper.  The  InvArm 
function  will  be  used  later  in  the  Forward  Arm. 
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DAC  Output 

Once  we  know  the  torques,  r,  that  wc  must  produce,  we  have  to  calculate  what  motor  voltages  we  must 
apply  in  order  to  generate  these  torques.  In  the  computer  program,  the  motor  voltage  calculation  is 
performed  separately  from  the  above  five  steps  because  when  the  Forward  Ann  uses  the  Inverse  Arm  it 
requires  the  torques  as  output  The  motor  voltages,  computed  from  the  torques  and  velocities,  can  be  used 
for  a  feedforward  compensation  control  system.  [2] 

We  assume  that  the  inductance  of  the  motors  is  negligible  so  that  the  equation  relating  voltage  to  the 
armature  resistance  and  motor  speed  is 

V  + 

where  R.  is  the  resistance  of  motor  /.  Kt.  is  a  back  EMF  constant  for  motor  /,  and  I.  is  the  current  in  motor  t. 
\  1  1 

The  torque  that  the  motor  produces  is  related  to  the  current  in  the  motor  and  is  given  by 
Ti  =  Kt.  I|- 

We  can  rewrite  these  equations  as 

V.  =  Rj  r4  /  Ktj  +  Kt4  if.  (11) 

Forward  Arm 

The  purpose  of  this  model  is  to  allow  us  to  simulate  the  movement  of  a  manipulator.  We  can  specify  the 
voltages  applied  to  each  of  the  motors  and  calculate  the  resulting  movement  of  the  arm.  The  Forward  Ann 
model  algorithm  is  taken  from  a  paper  on  manipulator  dynamic  simulation  written,  by  Walker  and  Orin.  [11] 
We  use  the  third  method  given  in  the  paper. 

The  Walker-Orin  algorithm  is  a  method  for  calculating  the  acceleration  of  each  joint  in  a  manipulator.  We 
use  a  third  order  Range- Kutta  integration  algorithm  to  compute  the  velocities  and  positions  of  the  joints  from 
the  accelerations.  We  have  added  a  model  of  the  motor  dynamics  so  that  motor  voltages  can  be  converted  to 
torques,  The  Forward  Arm  model,  which  consists  of  these  three  parts  (calculation  of  acceleration,  integration, 
and  motor  dynamics),  takes  as  input  a  voltage  schedule  which  is  a  list  of  input  voltages  to  be  applied  to  the 
motors  of  the  arm  over  a  period  of  time.  The  output  of  the  model  is  the  positions,  velocities,  and  accelerations 
that  the  joints  of  the  arm  undergo  with  the  specified  input. 

We  will  first  describe  the  motor  dynamics  equations  and  then  describe  the  Walker-Orin  algorithm  in  detail. 
This  discussion  will  be  completed  with  a  description  of  the  Runge-Kutta  algorithm  as  it  applies  to  this 
problem. 


Motor  Dynamics 

The  motors  have  characteristics,  such  as  back  EMF,  which  can  be  modeled  as  a  control  system  around  a 
Walker-Orin  arm  model,  (see  figure  2) 

The  voltages  applied  to  the  terminals  of  the  motors  have  the  back  EMFs  of  the  motors,  given  by  Kt 
subtracted  from  them.  The  result  is  multiplied  by  Ktj/Rj  to  give  the  torque  that  is  actually  generated  and 
applied  to  the  joint  of  the  arm.  The  inductance  of  the  motor  is  negligible  in  most  cases,  so  it  is  not  included  in 
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this  analysis.  The  torque,  in  terms  of  the  applied  voltage,  Vj,  and  the  joint  velocity,  0,  is  given  by 


Ti 


=  Ktj  (  Vj  -  Ktj  )  /  R. 


i  =  1,2,...,N. 


(12) 


*  0, 

*  0i 


Kt| 

Figure  2:  Control  System  Model  of  a  Motor 


The  Waiker-Orin  Simulation  Method 

The  dynamics  of  any  manipulator  can  be  summed  up  in  one  general  second-order  differential  equation.  [6] 
i  =H  (£)£  +  C(£,£)£  +  G(£)  +  ty*)7^  +  (13) 

where  H  ( £ )  is  an  N  x  N  symmetric  nonsingular  moment  of  inertia  matrix,  C  ( £ ,  £  )  is  an  N  x  N  matrix 
specifying  the  centrifugal  and  coriolis  effects,  G  (  £  )  is  a  vector  of  size  N  specifying  the  effects  of  gravity, 
Kf  (  £ )  and  Kn  (  £ )  are  3  x  N  Jacobian  matrices  specifying  the  torques  created  at  each  joint  due  to  external 
forces  and  moments  exerted  on  the  hand,  fhand  is  a  spatial  vector  specifying  the  external  force  exerted  on  the 
hand,  and  n“hand  is  a  vector  specifying  the  moments  exerted  on  the  hand  about  the  X,  Y,  and  Z  axes. 

The  purpose  of  this  pan  of  the  Forward  Arm  model  is  to  compute  the  accelerations  of  each  of  the  joints 
given  the  torques  applied  to  the  joints  and  the  current  state  (positions  and  velocities)  of  the  arm.  There  are 
three  parts  to  this  computation;  computing  the  bias  vector,  finding  the  H  matrix,  and  solving  for  the  joint 
accelerations.  The  Walker-Orin  paper  (11]  gives  four  methods  of  finding  the  accelerations.  The  first  and  third 
steps,  that  is  computing  the  bias  vector  and  solving  for  the  joint  accelerations,  are  common  to  the  first  three 
methods  presented  in  the  Walker-Orin  paper.  The  difference  between  the  three  methods  is  in  their 
algorithms  for  computing  the  H  matrix.  The  third  method  given  for  computing  the  H  matrix  is  used  here  for 
speed. 
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Computing  the  Bias  Vector 
If  we  let 

fi  =  C(4.4)4  +  0(4)  +  Kr(4>TLd  +  <14> 

then  equation  12  becomes 

H(£)£  =  (l-B).  (15) 

B  is  called  a  bias  vector  which  corresponds  to  the  torque  required  to  maintain  the  current  state  without  any 
acceleration.  The  bias  vector  can  be  computed  with  the  InvArm  function  by  setting  £  =  0  and  calling  the 
routine  to  calculate  the  torque.  If  wc  knew  the  H  matrix  then  we  could  solve  equation  15  as  a  set  of  N 
simultaneous  equations  in  N  unknowns.  The  £  would  be  the  unknowns. 


Calculating  the  H  matrix 

The  H  matrix  represents  an  effective  moment  of  inertia  for  the  arm.  It  is  a  function  entirely  of  the  arm 
position  since  the  velocity  effects  are  accounted  for  in  other  terms.  The  simplest  means  of  calculating  the  H 
matrix  is  to  set  one  element  of  £  to  1  and  all  of  the  rest  to  0.  We  can  use  the  InvArm  function  with  £  =  &  = 
G  ( £ )  =  0  to  compute  the  torque  vector  for  that  acceleration.  The  computed  torque  vector  is  equal  to  the 
column  of  the  H  matrix  corresponding  to  the  element  of  £  that  was  set  to  a  1  since  t  =  H  ( £ )  £  when  £  = 

G  ( £ )  =  k  =  0  from  equation  15.  This  is  Walker-Orin’s  method  1  which  is  simple,  but  computationally 
slow. 


Method  3  uses  a  different  approach.  The  same  assumption  about  the  acceleration  is  made,  but  the 
calculation  of  torques  proceeds  differently.  The  H  matrix  is  symmetrical  so  only  the  diagonal  and  top  half  of 
the  off  diagonal  elements  are  computed.  If£  is  set  up  as  before,  with  the/11  element  set  to  1  and  the  rest  0,  we 
notice  that  the  manipulator  can  be  viewed  as  a  singly  jointed  arm  with  a  "hand”  that  is  made  up  of  links  j 
through  N  and  the  base  made  up  of  links  0  through  ;  -/.  The  characteristics  of  the  "hand,"  its  mass,  center  of 
mass,  and  moment  of  inertia,  can  be  calculated  iteratively  using 


Mi 


Ci 


Mj+i  +  mj 

i 


-c-v;  +  MJ+lAr<vi+  pj+l  »i 

Ar‘E;tlAj+1  +  Mj+1[|Aj+1(cf+1+  pjtl)-(f  |JI 

-<aT‘<£1+  Pj+i)-'T)(Ai+1('T+i+  pJ+1>-cT)T] 


j  =  N-l 2,1 

j  =  N-l 2,1 


(16) 

(17) 


+  J*  +  mj[|S*-^|2I-(s*-^)(s*-^)T]  j  =  N-l . 2,1  (18) 

where  m.  is  the  mass  of  link  ;,  M.  is  the  mass  of  the  "hand"  that  is  links;  through  N  inclusive,  c*  is  the 
composite  center  of  mass  of  the  "hand,"  E*  is  the  composite  moment  of  inertia  of  the  "hand,"  and  I  is  the 
identity  matrix.  The  boundary  conditions  at  link  N  are 
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En  -  JN 


The  composite  mass,  M.,  is  the  sum  of  the  masses  of  links  j  through  N.  The  composite  center  of  mass,  c\  is 
computed  relative  to  the  origin  of  link  jin  link  j  coordinates,  c*  is  a  weighted  sum  of  the  center  of  mass  of 
link  j,  s’,  and  the  center  of  mass  of  composite  link  j+  /,  c*+  v  divided  by  the  total  mass  of  the  composite  link 
j ;  M..  s .  is  weighted  by  its  mass,  m.  and  c*_  l  is  weighted  by  M.+r  The  sum  of  the  position  vector,  p . + r 
which  points  from  the  link  j  origin  to  the  j+ 1  origin  and  cj+ 1  is  transformed  from  j+  /  coordinates  to  j 
coordinates  to  give  c?+  ^  relative  to  the  j  coordinate  origin  in  j  coordinates. 

The  composite  moment  of  inertia,  E*,  is  the  sum  of  the  moment  of  inertia  of  link  j,  J*.  and  the  composite 
moment  of  inertia  of  links  j  through  N,  E.  +  x,  both  moved  to  the  center  of  mass  of  composite  link  j.  E*+1must 
undergo  a  coordinate  transformation  before  it  is  added  to  J. . 


To  move  a  moment  of  inertia  tensor  we  use  a  form  of  the  parallel  axis  theorem  which  gives  the  moment  of 
inertia  of  an  object  around  an  arbitrary  location  when  the  moment  of  inertia  around  the  center  of  mass  is 
known.  [10]5 

I0  =I*  +  M[R2l-RRr] 

where  I*  is  the  moment  of  inertia  tensor  around  the  center  of  mass,  M  is  the  mass  of  the  object,  R  is  the  vector 
pointing  from  an  arbitrary  location  to  the  center  of  mass,  and  1  is  the  identity  matrix. 

E*+  Lis  first  transformed  to  j  coordinates  by  pre-  and  post-multiplication  by  Aj+ 1  and  its  inverse.  Then  it  is 
moved  to  a*.  R  is  a] + 1  ( c*  ,  +  p  .  + l )  -  cf  for  this  move  which  points  from  the  center  of  mass  of  composite 


link  j  to  die  center  of  mass  of  composite  link  j+  /.  J.  is  moved  to  the  center  of  mass  of  composite  link  j  by  the 
same  means.  R  is  s .  -  c*  in  this  case  which  is  a  vector  from  the  center  of  mass  of  composite  link  j  to  the 
center  of  mass  of  link  j. 


The  net  force  on  the  "hand"  is  the  force  acting  on  the  center  of  mass  which  is  the  "hand’s"  mass  times  its 
linear  acceleration.  Since  the  angular  acceleration  about  a  joint  axis  is  assumed  to  be  1,  the  linear  acceleration 
is  the  cross  product  of  the  angular  acceleration  vector  (which  is  just  the  Z  axis  of  link  j-l  expressed  in  j 
coordinates)  and  the  vector  from  the  axis  to  the  center  of  mass  of  the  link.  The  net  force  is  given  by 


R  =  Mj  [  Aj"1  ZQ  x  (c*+  p.)] 


j  =  N . 2,1 


The  net  moment  of  the  "hand"  is  the  moment  around  the  "hand’s"  center  of  mass  which  is  the  component  of 
the  moment  of  inertia  matrix,  E. .  which  is  in  the  direction  of  the  joint  axis,  or 


N.  =  E*  A]'1  ZQ  j  =  N . 2,1.  (20) 

The  force  that  link  j  -1  exerts  on  the  hand,  called  the  local  force,  is  equal  to  the  net  force  since  the  only  force 
exerted  on  the  "hand”  is  the  local  force.  The  moment  exerted  on  the  "hand"  by  link  j-l ,  called  the  local 
moment,  is  the  net  moment  plus  the  moment  produced  by  a  force  acting  at  a  distance  from  the  rotational  axis. 
The  force  is  the  net  force  and  the  distance  is  the  sum  of  the  center  of  mass  vector  of  link  j ,  c|*.  and  the  position 
vector,  p  j,  which  points  from  the  link  j -1  to  the  link  j  coordinate  axes. 
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fj  =Fj  j  =  N,...,2,l  (21) 

Sj  =  N.  +  (c*  +  pi )  x  ^  j  =  N.....2.1  (22) 

The  torque  required  in  the  joint  is  the  ( j,  j )  element  of  the  H  matrix  which  is  equal  to  the  component  of  the 
local  moment  in  the  direction  of  the  link  j  -I  Z  axis,  given  by 

Hjj  =  n.  •  (  Af1^)  j  =  N,...,l.  (23) 

This  gives  us  the  diagonal  of  the  H  matrix. 


The  off  diagonal  elements  can  be  computed  by  calculating  the  torques  needed  in  joints  /  through  j  -/  to 
maintain  this  static  situation.  The  local  forces  on  each  link  from  /  to;  -/  are  the  same  as  the  local  force,  f,  on 
link  j  when  transformed  to  the  correct  coordinate  system.  The  local  moment  of  link  /  is  the  sum  of  the  local 
moment  of  link  i+ 1  transformed  to  i  coordinates  and  the  moment  caused  by  the  local  force  of  link  i+ 1  acting 
at  a  distance  Pi+r 

ff  =Aj+1ff+1  i  =  j-1 — 1  (24) 


=  A!+1m 


p  i  x  (  a|+1  ) 


i  =  j-l,...,l. 


(25) 


*i+l  ■  Kj  -  V‘»j  »j+1. 

The  elements  of  the  off  diagonals  of  the  H  matrix  are  the  components  of  the  local  moments  which  are  in  the 
same  direction  as  the  Z  axes  of  the  previous  links. 


H,i  =Hji=it(^Z0) 


i  =  j-1.  ..,2.1 


j  =  N . 2,1. 


(26) 


Once  we  have  the  H  matrix  we  can  calculate  the  acceleration  vector  for  the  given  torque,  i,  and  computed 
bias  vector.  B  using  equation  15.  Since  the  H  matrix  is  symmetric  an  algorithm  tailored  to  such  matrices  is 
used  to  solve  the  simultaneous  equations.  [9] 


Defining  the  For  Arm  Function 

Like  the  InvArm  function  we  can  define  a  ForArm  function  which  returns  the  acceleration  of  the  arm  joints 
given  the  positions,  velocities,  and  joint  torques.  This  function  is  useful  in  explaining  the  Runge-Kutta 
integration  and  corresponds  to  a  real  function  in  the  modeling  software.  We  define  ForArm  as 

1 1  =  ForArm  (&4L  I ) 
where  £  £,  and  z  are  defined  in  section . 


Runge-Kutta  Integration 

The  ability  to  calculate  the  acceleration  for  a  given  set  of  conditions  allows  us  to  compute  future  positions 
and  velocities  of  the  joints  usipg  a  Runge-Kutta  integration  algorithm.  A  third  order  Runge-Kutta  provides 
enough  accuracy  without  sacrificing  too  much  speed. 

If  we  let 


F(fi,£.Y)  =  ForArm(£,£,[Kl(V-Ki£)/E]) 
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where  V  is  the  arm  vector  of  applied  motor  voltages  (see  section )  then  the  first  order  differential  equations 
that  we  are  trying  to  solve  are 

d(£) 

-  =  F(££Y) 

dt 

and 

d(£)  ■ 

-  =  £• 

dt 

To  use  the  third  order  Runge-Kutta  method,  we  calculate  two  sets  of  three  coefficients  which  represent  the 
current  values  and  estimated  future  values  of  £  and  £.  The  new  values  computed  for  £  and  £  are  the  old 
values  plus  a  time  increment  multiplied  by  a  weighted  sum  of  the  current  and  estimated  future  values  of  £ 
and  £.  [3]  The  coefficients  to  be  calculated  for  each  new  set  of  values  for  £  and  £  are 

=  £ 

ij  =  F(&  i  Y) 
h 

e2  =  £  +  —  k3 

k2  =  F(£  +  — -£j,  c2,  Y) 

C3  =  £  +  2hk2  -  h£x 

k3  =  F  ( £  +  2he2  -  hcr  s3,  Y) 

where  h  is  a  time  step.  The  determination  of  the  correct  size  for  the  time  step  h  is  a  difficult  procedure.  There 
arc  algorithms  for  adjusting  the  stepsizc  according  to  the  change  in  £  and  £,  but  these  are  not  currently 
implemented.  A  fixed  stepsize  of  1  millisecond  which  is  approximately  1/15*  of  the  smallest  mechanical  time 
constant  in  the  arm  is  currently  being  used. 

The  iterative  equations  for  calculating  the  weighted  sum  and  obtaining  new  values  for  £  and  £  are: 

£n+1  =  t  +  -(£1  +  4£2  +  £3) 

6 

£"  +  1  =  £n  +  -(^  +  4^  + *3) 

6 

Programs  Available 

We  have  implemented  the  model  of  arm  dynamics  which  has  been  described.  The  software  resides  on  the 
CMU-750R  VAX  in  the  /usnO/nms/sim  directory.  Currently,  the  software  is  in  the  form  of  an  executable 
testbed  called  "sim."  The  sim  program  is  Cl  based  with  full  help  functions.  The  sim  program  can  do  the 
Inverse  Arm  on  single  or  multiple  sets  of  data.  It  can  do  the  Forward  Arm  on  single  sets  of  data  or  do  a 
simulation  sequence. 
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The  sim  system  is  run  by  typing  /usr/nms/sim/sim.  The  user  is  then  at  the  Cl  user  interface  level.  There 

are  several  variables  which  the  user  can  set  to  different  values.  They  are 

gravity  The  value  of  the  gravitational  acceleration. 

Defaults  to  -9.80621  meters/second2 

DACTimePeriod  The  time  between  changes  in  the  reference  input  to  the 

control  system  of  the  arm 

The  following  variables  are  vectors  which  have  one  component  for  each  joint  in  the  arm  being  modeled. 

theta  Thejointangleofeachjointinthearm 

starting  from  the  base. 

omega  The  joint  angle  velocities.  This  is  the 

first  time  derivative  of  theta. 

alpha  The  joint  accelerations.  This  is  the  output 

of  the  Forward  Arm  model 

torque  The  torque  each  motor  is  producing.  This 

is  calculated  from  the  voltage. 

voltage  The  voltage  applied  or  that  should  be  applied  to 

each  joint  to  satisfy  the  other  conditions 

DACOut  The  Digital  to  Analog  Converter  output  voltage  which  is  connected  to 

the  reference  input  of  the  control  system  of  the  arm 

Any  of  these  variable  can  be  set  by  saying:  variable  =  valuel  value2  ...  valueN,  where  N  is  the  number  of 

joints  in  the  arm.  The  torque  and  voltage  variables  are  tied  together  so  that  setting  one  recalculates  the  other. 

The  commands  allow  the  user  to  apply  the  Inverse  Arm  or  Forward  Arm  models  to  different  sets  of  data. 

Possible  commands  are 

inverse.arm  Runs  the  Inverse  Arm  model  on  the 

theta,  omega,  and  alpha  variables.  The  result  is 
put  in  the  torque  and  voltage  variables. 

forward.arm  Runs  the  Forward  Arm  model  of  the 

theta,  omega,  and  voltage  variables.  The  result  is  put 
in  the  alpha  variable.  This  command  does  not  perform 
Runge-Kutta  integration. 
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simulate 


plot.simulation 


print.simulation 


write.graphic.file 


open.output.file 


close.output.file 


database.read 


Simulates  the  motion  of  an  arm.  Prompts  the 
user  as  to  whether  it  should  start  a  new  simulation, 
set  the  theta  and  omega  variables  to 
zero  before  doing  a  simulation,  and  whether  a  voltage 
schedule  file  is  used.  If  a  voltage  schedule  file  is 
used  the  user  is  prompted  for  its  name.  If  a  schedule 
file  is  not  used  the  voltage  is  assumed  to  W  held 
constant  at  the  value  given  by  the  voltage  variable 
and  the  user  is  prompted  for  the  number  of  timesteps 
over  which  the  simulation  is  to  occur. 

Plots  the  simulation  on  the  dover. 

Generates  a  poof  file  (see  MAN  POOF)  called 
tfdoverploLpoof  and  a  press  file  called  #poof.press. 

Prints  the  simulation  on  the  user’s 

screen  if  no  file  is  currently  open.  Prompts  the  user 

for  start,  finish,  and  step  values. 

Writes  a  file  of  simulation  results 
which  is  suitable  for  input  to  the  graphics 
simulation  display  package  on  the  PERQ. 

Opens  a  file  for  writing  with 
print.simulation.  Prompts  user  for  filename. 

Closes  the  open  file  used  for 

simulation  output  Further  output  is  sent  to  the 

terminal  after  the  file  is  closed. 

Reads  a  database  file  for  an  arm  which  contains 
'  mass,  center  of  mass,  moment  of  inertia, 
Denavit-Hartenburg  parameters,  and  other  related  factors 
which  characterize  an  arm.  This  command  is  done  implicitly 
for  the  CMU  DD  Arm  when  the  sim  program  is  started  up. 


Further  information  about  the  internal  workings  of  the  system  is  in  the  program  documentation. 


Arm  Dynamics  Simulation 


16 


23  November  1982 


General  Equation  of  Arm  Dynamics 

1  =H(£)£  +  +  C(£)  +  Kf(£)T4nd  +  <13> 

B  =  €(£,£)£  +  G(fi)  +  K  +  V*)7^  (14) 

H(A)i  =  (l*B)  (15) 

Where: 


H(£) 

C(£,£) 

G(£) 

Kf(£) 


&  An  NxN  symmetric  nonsingufar  moment  of  inertia  matrix. 

£  An  NxN  matrix  specifying  the  centrifugal  and  coriolis  effects. 

£  An  arm  vector  specifying  the  effects  due  to  gravity. 

£  A  3xN  Jacobian  matrix  specifying  the  torques  created  at  each 
joint  due  to  an  external  force  exerted  on  the  hand. 


V4>  £  A  3xN  Jacobian  matrix  specifying  the  torques  created  at  each 

joint  due  to  an  external  moment  exerted  on  die  hand. 


£  A  spatial  vector  specifing  the  force  exerted  on  the  hand  externally. 


nhand 


£  A  vector  specifying  the  external  moments  around  the  X,  Y,  and  Z  axes. 


B 


£  A  bias  vector  which  can  be  computed  by  equations  1  through  10  with  £ 
set  equal  to  0. 


Arm  Dynamics  Simulation 


17 


23  November  1982 


Equations  for  Evaluating  the  Dynamic  Motion  of  a  Manipulator 


"i+l 

= 

A!+i  <  «i  + 

i  =  0,1 . N-l  (1) 

"i+l 

= 

A!+i (  «i  +  Vi+1  +  «t  x  Vi+i ) 

i  =  0,1,...,N-1  (2) 

Vi 

= 

"i+ix  pi+i  +  AS+i^ 

i  =  0,1 . N-l  (3) 

Vi 

= 

5i+i  x  pi+i  +  «i+i  x  < "i+i  x  pi+i>  +  A!+i  \ 

i  =  0,1,...,N-1 

(4) 

v. 

1  . 

= 

WjXS*  +  Uj  X  (  Mj  X  S*)  +  Vj 

i  =  1,2,...,N 

(5) 

F. 

1 

= 

mivi 

i  =  1,2,...,N 

(6) 

N. 

= 

jj  Wj  +  Wj  X  (  Jj  ) 

i  =  1,2,...,N 

(7) 

t 

1 

= 

Ai*‘  tl  +  F. 

i  =  N . 2,1 

(8) 

— 

a;+1^,+  P,x(A|*1t1)  +  (P,+  + 

i  =  N . 2,1 

(9) 

Ti 

= 

VK'V  + 

i  =  N-l . 1 

(10) 

Where: 

,  w 


vvi 


F..N. 

I  1 


f ,  a. 

i  *  i 


o.,ere. 


Pi 

«.  • 
S: 


J, 


a  The  angular  velocity  and  acceleration  of  the  link  i  coordinate  system, 
a  The  linear  velocity  and  acceleration  of  the  link  i  coordinate  system, 
a  The  linear  acceleration  of  the  center  of  mass  of  link  i. 
a  The  net  force  and  moment  exerted  on  link  /'. 
a  The  force  and  moment  exerted  on  link  /  by  link  i  -  /. 
a  The  input  torque  for  joint  /. 
a  A  number  representing  viscous  damping  or  friction, 
a  The  angle,  and  its  derivatives,  through  which  joint  /  is  turned, 
a  Vector  from  link  i  - 1  origin  to  link  / coordinate  system  origin, 
a  Vector  from  link  i  coordinate  system  origin  to  center  of  mass  of  link  /. 
a  Moment  of  inertia  matrix  of  link  /. 


m. 


a  Mass  of  link  L 
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Given: 

*!+1  = 


cos  6 
sin  6 
0 


i+l 

i+1 


-cosai+1sin<?i+1 

cosai+1cosdi+1 


sinai+1sindi+1 
-sin  «i+1cos0i+1 


sin  a 


i+l 


cos  a. 


i+l 


A^+ 1  =  I,  The  identity  matrix 

The  N  +  1th  coordinate  system  is  the  hand  coordinate  system  which  is 
identical  to  the  link  N  coordinate- system. 


rcosCctj) 


Zq  =  [001]T 

“o’^o-^o  =  l°001T 

*o  =  [0  0  g]T 


g  =  9.80621  meters/sec2  if  the  Z-axis  of  the  link  0  coordinate  frame  is 
pointing  vertically  up. 


g  =  -  9.80621  meters/sec2  if  the  Z-axis  of  the  link  0  coordinate  frame 
is  pointing  vertically  down. 


fN  +  L  a  The  external  force  on  the  hand. 
nN+1  £  The  external  moment  on  the  hand. 
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Equations  for  Calculation  of  H  Matrix  in  Forward  Arm  Problem 


Mj  =  M.+1  +  m. 


+  Mi*iAi+'<Vi+ 

Ei'=  aT%iaU  +  MI+inAr'<v^  v.Ki2i 
-<Ar<v.+  pJ+,)-f)(V1‘Vi+  5w)-f>T] 

+  j-  +  mJus;-^|2i-(5;-f)(3;-^)T] 

Fj  =  Mj  t  Aj'1  ZQ  x  (c*  +  p.)] 

Nj  =  e;  Af1  z„ 


j  =  N-l,...,2,l  (16) 
j  =  N-1.....2.1  (17) 


fi  =Fi 


nj  =  Nj  +  (cf  +  Pj)  x  Fj 

Hii  =  vKlv 


f  —  Ai+1  T 

J  1  1+1 


"i =  Ai+1"i*i +  Pix<Ai+1fltl) 


Hu  =  Hii  = g,  ■  < ASl z» > 


i  =  j-1 . 2,1 


where 


The  cumulative  mass  of  links  j  through  N  with  MN  =  mN. 


j  =  N-l,...,2,l  (18) 
j  =  N,...,2,l  (19) 


j  =  N . 2,1  (20) 


j  =  N-l . 2,1  (21) 


j  =  N . 2.1  (22) 


j  =  N-l . 1  (23) 


i  =  j-1 1  (24) 

i  =  j-1 1  (25) 


j  =  N . 2,1  (26) 


a  The  center  of  mass  of  composite  link  j  with  c^*  =  s  ^ 


*  The  moment  of  inertia  of  composite  link  j  around  its  center  of  mass  with  E*  =  J* 

N  N 

a  The  N  X  N  matrix  relating  angular  accelerations  to  joint  torques. 
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